#include "byteblock.h"
#include "settings.h"


#ifdef DEBUG
#include "HardwareSerial.h"
#endif


/*
  ------------
 |  Helpers:  |
  ------------
*/

#ifdef DEBUG
void binprint(const byte &var) {
  byte mask = 0b10000000;
  while (mask) {
    if (mask & var)
      Serial.print("I ");
    else
      Serial.print("O ");
    mask >>= 1;
  }
  Serial.println();
}
#endif


byte reverse_byte(const byte &var) {
  byte result = 0;
  byte mask = 0b00000001;

  for (uint8_t i=0; i<8; i++) {

    if (var&mask)
      result |= 1;
    mask <<= 1;
    if (mask)
      result <<= 1;
  }

  return result;
}



/*
  ------------------
 |  Class Methods:  |
  ------------------
*/

#ifdef DEBUG
void ByteBlock::print(void) {
  byte mask;

  Serial.println("-------------------------------");
  for (uint8_t row=0; row<8; row++) {
    mask = 0b10000000;
    while (mask) {
      if (mask & this->arr[row])
        Serial.print("I ");
      else
        Serial.print("  ");
      mask >>= 1;
    }
    Serial.println("");
  }
  Serial.println("\n-------------------------------");
}
#endif


ByteBlock::ByteBlock(const byte *new_arr) {
  for (uint8_t i=0; i<8; i++)
    this->arr[i] = new_arr[i];
}

ByteBlock::ByteBlock(const byte b1, 
                     const byte b2, 
                     const byte b3, 
                     const byte b4, 
                     const byte b5, 
                     const byte b6, 
                     const byte b7, 
                     const byte b8) {
  this->arr[0] = b1;
  this->arr[1] = b2;
  this->arr[2] = b3;
  this->arr[3] = b4;
  this->arr[4] = b5;
  this->arr[5] = b6;
  this->arr[6] = b7;
  this->arr[7] = b8;
}

ByteBlock::ByteBlock(const ByteBlock &other_bb) {
  for (uint8_t i=0; i<8; i++)
    this->arr[i] = other_bb.arr[i];
}

void ByteBlock::mirror_lefttright() {
  for (uint8_t row=0; row<8; row++) {
    this->arr[row] = reverse_byte(this->arr[row]);
  }
}

void ByteBlock::mirror_updown() {
  byte temp;

  for(uint8_t i=0; i<4; i++) {
    temp = this->arr[7-i];
    this->arr[7-i] = this->arr[i];
    this->arr[i] = temp;
  }
}

void ByteBlock::rotate_180() {
  byte temp;

  for(uint8_t i=0; i<4; i++) {
    temp = reverse_byte(this->arr[7-i]);
    this->arr[7-i] = reverse_byte(this->arr[i]);
    this->arr[i] = temp;
  }
}

void ByteBlock::rotate_90_right(void) {
  byte temp_arr[8] = {0};
  byte mask = 0b10000000;

  for (uint8_t x=0; x<8; x++) {
    for (uint8_t y=0; y<8; y++) {
      if (this->arr[y] & mask) {
        temp_arr[x] |= 0b10000000;
      }
      if (y<7)
        temp_arr[x] >>= 1;
    }
    mask >>= 1;
  }

  for (uint8_t row=0; row<8; row++) {
    this->arr[row] = temp_arr[row];
  }
}

void ByteBlock::rotate_90_left(void){
  byte temp_arr[8];
  byte mask;

  for (uint8_t result_row=0; result_row<8; result_row++) {
    mask = 0b00000001 << result_row;

    for (uint8_t col=0; col<8; col++) {
      if (this->arr[col] & mask) {
        temp_arr[result_row] |= 1;
      }
      if (col < 7)
        temp_arr[result_row] <<= 1;
    }
  }
  
  for (uint8_t row=0; row<8; row++) {
    this->arr[row] = temp_arr[row];
  }
}

#ifdef DEBUG
void test_byteblock(void) {
  ByteBlock bb(
    0b10000000,
    0b11000000,
    0b10100000,
    0b10010000,
    0b10001000,
    0b10000100,
    0b10000010,
    0b10000001
  );

  Serial.print("Testing Reverse Byte.");
  Serial.print("    Orig: ");
  binprint(0b11110000);
  Serial.print("Reversed: ");
  binprint(reverse_byte(0b11110000));
  Serial.println("-------------------\n");


  Serial.println("Original Byteblock:");
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Testing Rotate Right:");
  bb.rotate_90_right();
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Rotate Left back to normal:");
  bb.rotate_90_left();
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Rotate 180 degrees:");
  bb.rotate_180();
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Mirror UpDown Axix (Works!):");
  bb.mirror_updown();
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Mirror LeftRight Axix:");
  bb.mirror_lefttright();
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Creating Byteblock from swastica char[] array:");
  byte swastika[] = {24, 48, 38, 155, 217, 100, 12, 24};
  bb = ByteBlock(swastika);
  bb.print();
  Serial.println("-------------------\n");

  Serial.println("Testing Byteblock Done!");
}
#endif